#include <Servo.h>
#include <ESP8266WiFi.h>

WiFiServer server(10086);

//后臂长度（单位mm）
double rlen = 80.0;

//钳子长度（单位mm）
double clen = 50.0;

//底盘舵机
Servo base;

//后臂舵机Rear Arm（右）
Servo rarm;

//前臂舵机Fore Arm（左）角度40 - 120
Servo farm;

//钳子舵机 Claw
Servo claw;

//角度延时
int adelay = 5;

//舵机速度控制
//参数1：舵机
//参数2：起始角度
//参数3：目标角度
void speed_control(Servo& s, int begin, int end)
{
  //逆时针旋转
  if (end > begin)
  {
    for (int i = begin; i <= end; i++)
    {
      s.write(i);
      delay(adelay);
    }
  }
  //顺时针旋转
  if (end < begin)
  {
    for (int i = begin; i >= end; i--)
    {
      s.write(i);
      delay(adelay);
    }
  }
}

int joint1_read()
{
  return (90 - base.read());
}

//关节 Joint
void joint1_write(int angle)
{
  if (angle > 90)
  {
    angle = 90;
  }

  if (angle < -90)
  {
    angle = -90;
  }
  Serial.println("关节1从" + String(joint1_read()) + "旋转到" + String(angle));
  //读取舵机当前角度
  int begin = base.read();
  //计算舵机目标角度
  int end = 90 - angle;
  
  Serial.println("底座舵机从" + String(begin) + "旋转到" + String(end));
  //控制旋转速度(5ms/度)
  speed_control(base, begin, end);
}

//读取关节2旋转角度
int joint2_read()
{
  return 180 - rarm.read();
}

void joint2_write(int angle)
{
  if (angle < 0)
  {
    angle = 0;
  }

  if (angle > 130)
  {
    angle = 130;
  }
  Serial.println("关节2从" + String(joint2_read()) + "旋转到" + String(angle));
  int begin = rarm.read();
  int end = 180 - angle;
  Serial.println("后臂舵机（右）从" + String(begin) + "旋转到" + String(end));
  speed_control(rarm, begin, end);
}

int joint3_read()
{
  return farm.read() - joint2_read() + 90;
}

void joint3_write(int angle)
{
  Serial.println("关节3从" + String(joint3_read()) + "旋转到" + String(angle));
  int lambda = joint2_read() + angle - 90;
  //限制舵机的旋转角度
  if (lambda < 40)
  {
    lambda = 40;
  }

  if (lambda > 120)
  {
    lambda = 120;
  }
  Serial.println("前臂舵机（左）从" + String(farm.read()) + "旋转到" + String(lambda));
  speed_control(farm, farm.read(), lambda);
}

//弧度转角度 radian
int rad2ang(double r)
{
  return r * 180 / PI;
}

void sety(int y)
{
  int theta2 = rad2ang(acos(y / (2 * rlen)));
  int theta3 = 2 * (90 - theta2);
  Serial.println("theta2: " + String(theta2));
  joint2_write(theta2);
  Serial.println("theta3: " + String(theta3));
  joint3_write(theta3);
}

//计算斜边长度 hypotenuse
double hypot(double x, double y)
{
  return sqrt(x*x + y*y);
}

void setxy(int x, int y)
{
  //计算坐标和原点的距离
  double d = hypot(x, y);
  Serial.println("坐标点和原点距离" + String(d));
  sety(d);
  //计算关节1的旋转角度
  //C语言中整数除法的结果不保留小数部分
  //需要将除数和被除数转为浮点类型
  double x1 = x;
  double y1 = y;
  int theta1 = rad2ang(atan(x1/y1));
  Serial.println("关节1旋转角度" + String(theta1));
  joint1_write(theta1);
}

//机械臂当前坐标
int x = 0;
int y = 80;
int z = 80;
int e = 0;
int ox = x;
int oy = y;
int oz = z;

//定位到空间中一点，Z轴原点为底座高度，只处理Z轴坐标为0到正数的情况
void setXYZ(Stream& s, int cx, int cy, int cz)
{
  output(Serial, 'X', cx);
  output(Serial, 'Y', cy);
  output(Serial, 'Z', cz);
  Serial.println();
  //在机械臂方向减去钳子长度
  double d = hypot(cx, cy) - clen;
  double d1 = hypot(d, cz);
  int theta21 = rad2ang(acos((d1*d1)/(2*d1*rlen)));
  int theta22 = rad2ang(atan(cz/d));
  int theta2 = theta21 + theta22;
  int theta3 = rad2ang(acos((2*rlen*rlen - d1*d1)/(2*rlen*rlen)));
  double x1 = cx;
  double y1 = cy;
  int theta1 = rad2ang(atan(x1/y1));
  //s.println("关节1旋转角度" + String(theta1));
  //s.println("关节2旋转角度" + String(theta2));
  //s.println("关节3旋转角度" + String(theta3));
  joint1_write(theta1);
  joint2_write(theta2);
  joint3_write(theta3);
}

//线性插值
void linear_move(Stream& s)
{
  double d = hypot(hypot(x - ox, y - oy), z - oz);
  int r = round(d);
  if (r)
  {
    for (int i = 0; i <= r; i++)
    {
      setXYZ(s, ox + (x - ox)*i/r, oy + (y - oy)*i/r, oz + (z - oz)*i/r);
      delay(adelay);
    }
  }
}

//控制钳子角度
void setClaw()
{
  if (e < 0)
  {
    e = 0;
  }

  if (e > 180)
  {
    e = 180;
  }
  int begin = claw.read();
  int end = map(e, 0, 180, 38, 60);
  Serial.println("钳子舵机从" + String(begin) + "旋转到" + String(end));
  speed_control(claw, begin, end);
}

void help(Stream& s)
{
  s.println("机械臂命令：");
  s.println("G00 [X] [Y] [Z] [E] 快速定位");
  s.println("G01 [X] [Y] [Z] [E] [F] 线性移动");
  s.println("所有命令以换行结束，单位mm");
}

void setup() {
  base.attach(D5, 500, 2500);
  joint1_write(0);
  rarm.attach(D6, 500, 2500);
  joint2_write(90);
  farm.attach(D7, 500, 2500);
  joint3_write(90);
  claw.attach(D8, 500, 2500);
  claw.write(50);
  //等待舵机稳定
  delay(1000);

  Serial.begin(74880);
  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(LED_BUILTIN, HIGH);
  WiFi.mode(WIFI_STA);
  WiFi.begin("taromob", "12345678");
  while (!WiFi.isConnected())
  {
    delay(100);
  }
  Serial.println("WiFi已连接，IP地址" + WiFi.localIP().toString());
  //禁用TCP的Nagle算法，降低客户端时延
  server.setNoDelay(true);
  //启动服务器
  server.begin();
  digitalWrite(LED_BUILTIN, LOW);
  Serial.println("机械臂已启动");
}

//解析命令参数
//参数1：要解析的命令字符串
//参数2：解析的参数名
//参数3：如果没有找到参数，返回的默认值
//G00 X111 Y2 Z3 E0
int parseArg(String cmd, char name, int defval)
{
  int i = cmd.indexOf(name);
  if (i == -1) //没有找到指定的参数
  {
    //返回默认值
    return defval;
  }
  //将命令后的数字字符转为整数类型
  return cmd.substring(i+1).toInt();
}

//输出当前坐标
void output(Stream& s, char name, int value)
{
  s.print(name);
  s.print(value);
  s.print(" ");
}

//快速移动处理
void G00(Stream& s, String cmd)
{
  adelay = 5;
  x = parseArg(cmd, 'X', x);
  y = parseArg(cmd, 'Y', y);
  z = parseArg(cmd, 'Z', z);
  e = parseArg(cmd, 'E', e);
  setXYZ(s, x, y, z);
  setClaw();
  output(s, 'X', x);
  output(s, 'Y', y);
  output(s, 'Z', z);
  output(s, 'E', e);
  s.println("ABS");
  s.println("OK");
}

//线性移动处理
void G01(Stream& s, String cmd)
{
  //保存XY初始坐标，用于线性插值
  ox = x;
  oy = y;
  oz = z;
  x = parseArg(cmd, 'X', x);
  y = parseArg(cmd, 'Y', y);
  z = parseArg(cmd, 'Z', z);
  e = parseArg(cmd, 'E', e);
  adelay = parseArg(cmd, 'F', 2);
  linear_move(s);
  setClaw();
  output(s, 'X', x);
  output(s, 'Y', y);
  output(s, 'Z', z);
  output(s, 'E', e);
  s.println("ABS");
  s.println("OK");
}

void loop() {
  WiFiClient client = server.available();
  if (!client)
  {
    return;
  }

  Serial.println("客户端" + client.remoteIP().toString() + "已连接");
  help(client);
  
  while (client.connected())
  {
    if (client.available())
    {
      String line = client.readStringUntil('\n');
      if (line.startsWith("G00")) G00(client, line);
      if (line.startsWith("G01")) G01(client, line);
      //if (line.startsWith("G04")) G04(client, line);
    }
  }
}
